/*
 *  constraint.h
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 25/08/2011
 *
 */

#ifndef _constraint_h_
#define _constraint_h_

#include <string>
#include <map>
//#include <algorithm>
#include "point.h"
//#include "MATERIALS/material.h"
//#include "sparsematrixslu.h"
#include "commonutil.h"
//#include "TENSOR/tensor.h"
//#include "TENSOR/xtensor.h"
//#include "TENSOR/tutils.h"
#include "Eigen/Dense"

//class Body;

using namespace std;
using namespace Eigen;

class Constraint {
public:
	int dim;					/** Dimension */
	int numDofs;				/** Number of dofs */
	vector<string> pointNames;						/** Names of the points belonging to the Constraint */
	map<string,Point*,less<string> > mPoints;		/** Pointers to points belonging to the Constraint */
	// WARNING: pointNames and mPoints have different ordering
	vector<int> localGlob;		/** Local-global numbering */
	double time;				/** Time (local) */
	double deltaT;
	double penalty;
	VectorXd q;
	VectorXd dq;
	VectorXd qMid;
	VectorXd qC;
	VectorXd dqC;
	VectorXd Fq;
	MatrixXd Kq;
	MatrixXd Kdq;
	double Phi;			// Constraint
	double PhiC;		// Coverged Constraint
	VectorXd dPhi;		// Constraint gradient
	MatrixXd ddPhi;		// Constraint Hessian
	VectorXd FqLag;
	VectorXd FqPenalty;
	VectorXd deltaQ;
	VectorXd discretedPhi;
	double perturbation;			/** For numerical differentiation */
	int integratorType;
	double lagMul;	// Lagrange multiplier
	bool augLagFlag, augConvergence;
	double FqRatio;
	bool isQuadratic;
	bool isContact;
	bool isScalar;
	double midCorrection;
	double averagePhi;
	double denom;
	double midcorrection;
	vector<int> numDofsPoints;
	VectorXd vaux;

public:
	Constraint();
	Constraint(vector<string>, int);
	virtual ~Constraint(){}

	vector<string>& getPointNames();
	void setMPoints(map<string,Point*,less<string> >);
	void reportContent();
	void fillResidual(vector<double>& res_in, 
					  const vector<double>& q_in,
					  const vector<double>& dq_in, 
					  const double& t, 
					  const int& modeFlag, 
					  const double& grav,
					  const int& gravDir,
					  const double& DeltaT);
	void fillTangent(sparseMatrix&, vector<vector<double> >&, vector<vector<double> >&, vector<vector<double> >&,
					 vector<int>&, vector<int>&,
					 const vector<double>& q_in,
					 const vector<double>& dq_in,
					 const double& time, const int&, const double deltaT);
	void fillForce(vector<double>& f,
						const vector<double>& q,
						const vector<double>& qdot,
						const double& t,
						const double&, const int&
					);

	void getLocalDofs(const vector<double>&, const vector<double>&);	/** Receive the global vector and extract local values */
	void setLocalGlob();												/** Set local-global numbering */
	void setSparsePattern(vector< vector<int> >&);
	void changePointName(string&, string&);
	void calcKqNumeric();
	void update(const int&, const vector<double>&, const vector<double>&);
	void updateLagMul(void);
	void checkConvLagMul(bool&);
	void calcProjMatrix(sparseMatrix&, double&);
	void calcCon(vector<double> &, vector<double>&, vector<double>&, vector<double>&, double&);
	void fillPhiq(vector<vector<double> >&, int&);
	void setPointNames(vector<string>&);
	void calcFq(const int&);
	void calcKq(int);
	void calcKdq(double&);
	void initializeConsRes(const bool&, const double&, const int&, const bool);
	void initializeConsTang(const bool&);
        void calcEner(double&, double&, double&, double&, double&, double&, double&, int&, vector<double>&, vector<double>&);
	void initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double & t);			/** Set initial dof values */
	void setDofs(void);
	void calcDiscreteDPhi(VectorXd &, VectorXd &);
	MatrixXd calcDDiscreteDPhi(VectorXd &, VectorXd &);
	
	/** Virtual functions */
	
	virtual void calcPhi(const VectorXd &)=0;
	virtual void calcDPhi(const VectorXd &)=0;
	virtual void calcDDPhi(const VectorXd &)=0;
	virtual void setFqSymmetries()=0;
	virtual void setKqSymmetries()=0;
	virtual void calcFqC(const int&){};
	virtual void calcKqC(int){};
	virtual void calcKdqC(double&){};
	virtual void calcEnerC(double& Ekin, double& Epot, double& Econ, double& Eamb, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in){};
	virtual void initializeConsResC(const bool&, const double&, const int&, const bool){};
	virtual void initializeConsTangC(const bool&){};
	virtual void updateCons(){};
	virtual double getScalar(const VectorXd &){};
	virtual VectorXd getDScalar(const VectorXd &){};
	virtual MatrixXd getDDScalar(const VectorXd &){};
	virtual void setParamAtCons(vector<double>&){};

	
	/** Friend functions */
//	friend void evalMid(vector<double>& vMid, vector<double>& vN, vector<double>& vNPlus);

};
#endif
